#include <cstdlib>
#include "GraphicsMain.h"
#include "AnimationLoader.h"
#include "AnimNui.h"
#include "macros.h"
#include "Eigen/Geometry"
#include <time.h>
#include <windows.h>
#include "Cell.h"
#include <cmath>
#include "Gesture.h"
#include "imageloader.h"
#include <fstream>
#include "XmlMap.h"
#include "Character.h"
#include "Skybox.h"
#include "CollDetection.h"

#include <iostream>

using namespace std;

typedef Animation* AnimationList;
extern AnimationList* animationList;
//extern AnimNui kinectInterface;
extern string skeletalNodeNames[];
extern Wall collidedWall;
Character character;

#define TOTAL_ANIMATION_FILES 6
const float speed = 0.4f;
const float angularSpeed = 1.0f;

namespace GraphicsMain
{
	//Coordinate System of Screen (Projection)
	CoordinateSystem	coord;	
	float theta = 0.0f;	
	Cell labyrinth;
	Skeleton currentSkeleton[NUI_SKELETON_COUNT];
	vector<vector<float>> cellVector;
	Gesture* gestureProcessor;
	SkyBox sky;

	//3 for denied nodes
	const int indiceCount = (SKELETAL_NODE_COUNT - 1) * 2;

	const SkeletalNodes drawIndices[indiceCount] = 
	{
		nodeHead,			nodeShoulderCenter,
		nodeShoulderCenter, nodeShoulderLeft,
		nodeShoulderLeft,	nodeElbowLeft,
		nodeElbowLeft,		nodeWristLeft,
		nodeWristLeft,		nodeHandLeft,
		nodeShoulderCenter, nodeShoulderRight,
		nodeShoulderRight,  nodeElbowRight,
		nodeElbowRight,		nodeWristRight,
		nodeWristRight,		nodeHandRight,
		nodeShoulderCenter, nodeSpine,
		//nodeHipCenter,		nodeSpine,   
		nodeSpine,			nodeHipLeft,
		nodeHipLeft,		nodeKneeLeft,
		nodeKneeLeft,		nodeAnkleLeft,
		//nodeAnkleLeft,		nodeFootLeft,
		nodeSpine,			nodeHipRight,
		nodeHipRight,		nodeKneeRight,
		nodeKneeRight,		nodeAnkleRight,
		//nodeAnkleRight,		nodeFootRight
	};

	void Initializer()
	{
		glClearColor (0.0, 0.0, 0.0, 1.0);
		glEnable(GL_LIGHTING);
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHT1);
		glEnable(GL_NORMALIZE);
		glEnable(GL_DEPTH_TEST);
		glEnable(GL_CULL_FACE);	

		coord.xMin = -20.0f;
		coord.xMax =  20.0f;
		coord.yMin = -20.0f;
		coord.yMax =  20.0f;
		coord.zMin =  1.0f;
		coord.zMax =  2000.0f;

//		kinectInterface.Initialize();

		gestureProcessor = new Gesture();

		for(int i = 0; i < NUI_SKELETON_COUNT; i++)
		{
			for(int j = 0; j < SKELETAL_NODE_COUNT; j++)
			{
				Vector4f zero(0.0f,0.0f,0.0f,0.0f);
				currentSkeleton[i].addSkeletalNode((SkeletalNodes) j," ",zero);
			}	
		}	

		 gameInitializer();
		 skyBoxInitializer();
	}

	void deInitializer()
	{
		if(animationList)
		{
			for (int i = 0; i < TOTAL_ANIMATION_FILES; i++)
			{
				delete animationList[i];
			}

			delete animationList;
		}	

		if (gestureProcessor)
		{
			delete gestureProcessor;
		}

//		kinectInterface.DeInitialize();
	}

	void controllerFunction(unsigned char key, int x, int y)
	{
		if(key == 27)
		{
			deInitializer();
			exit(0);
		}

		if (key == 'w')
		{
			move(speed,0.0f);
		}

		if (key == 's')
		{
			move(-speed,0.0f);
		}

		if (key == 'a')
		{
			move(0.0f,-angularSpeed * 2.0f);
		}

		if (key == 'd')
		{
			move(0.0f,angularSpeed * 2.0f);
		}

		if (key == 'o')
		{
			theta -= 2.0f;
		}

		if (key == 'l')
		{
			theta += 2.0f;
		}
	}

	void reDraw()
	{
		glLoadIdentity();
		unsigned long now = GetTickCount();				
		glutPostRedisplay();

	//	theta+= 5.0f;

		if(theta > 360.0f)
			theta -= 360.0f;

	/*	kinectInterface.Nui_ProcessThread();

		for (int i = 0; i < NUI_SKELETON_COUNT; i++)
		{
			if(kinectInterface.currentSkeleton[i])
			{
				gestureProcessor->processGesture(kinectInterface.currentSkeleton[i]);
				int state = gestureProcessor->getNavigation(kinectInterface.currentSkeleton[i]);

				switch(state) 
				{
				case NO_HAND:
						break;
				case LEFT_HAND:
						move(0.0f,-angularSpeed);
						break;
				case RIGHT_HAND:
						move(0.0f,angularSpeed);
						break;
				case BOTH_HAND:
						move(speed,0.0f);	
						break;
				case LEFT_HAND_MOVE:
						move(speed,-angularSpeed);	
						break;
				case RIGHT_HAND_MOVE:
						move(speed,angularSpeed);	
						break;
				}

				for(int k = 0, j = 0; k < SKELETAL_NODE_COUNT + DENIED_NODE; k++)
				{
					if(!AnimationLoader::denyList( (SkeletalNodes) k))
					{
						float x,y,z,w;

						x = kinectInterface.currentSkeleton[i]->SkeletonPositions[k].x;
						y = kinectInterface.currentSkeleton[i]->SkeletonPositions[k].y;
						z = kinectInterface.currentSkeleton[i]->SkeletonPositions[k].z;
						w = 1.0f;

						currentSkeleton[i].setSkeletalNode((SkeletalNodes) j,x,y,z,w);
						j++;
					}		
				}	
			} 	
		}		*/	
	}

	void screenReshape(int width, int height)
	{
		if(height == 0) 
			height = 1;

		float ratio = (1.0 * width) / height;

		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glViewport(0, 0, width, height);
		gluPerspective(25,ratio,coord.zMin,coord.zMax);

		glMatrixMode(GL_MODELVIEW);
		gluLookAt( 0.0f, 0.0f,  1.0f,		
				   0.0f, 0.0f,  0.0f,		
				   0.0f, 1.0f,  0.0f);
	}

	void drawScene() {

		//Clear information from last draw
		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);						

		//Add ambient light
		GLfloat ambientColor[] = {0.4f, 0.4f, 0.4f, 1.0f}; //Color(0.2, 0.2, 0.2)
		glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientColor);

		//Add positioned light
		GLfloat lightColor0[] = {0.5f, 0.5f, 0.5f, 1.0f}; //Color (0.5, 0.5, 0.5)
		GLfloat lightPos0[] = {4.0f, 0.0f, 8.0f, 1.0f}; //Positioned at (4, 0, 8)
		glLightfv(GL_LIGHT0, GL_DIFFUSE, lightColor0);
		glLightfv(GL_LIGHT0, GL_POSITION, lightPos0);

		//Add directed light
		GLfloat lightColor1[] = {0.6f, 0.6f, 0.8f, 1.0f}; //Color (0.5, 0.2, 0.2)
		//Coming from the direction (-1, 0.5, 0.5)
		GLfloat lightPos1[] = {-1.0f, 0.5f, 0.5f, 0.0f};
		glLightfv(GL_LIGHT1, GL_DIFFUSE, lightColor1);
		glLightfv(GL_LIGHT1, GL_POSITION, lightPos1);	
		
		drawSkybox();
		drawLabyrinth();
		
		glutSwapBuffers();
	}

	void GraphicsMainFunction(int argc, char* argv[])
	{
		glutInit(&argc,argv);
		glutInitDisplayMode (GLUT_DOUBLE | GLUT_RGB);
		glutInitWindowSize(1280, 720);
		glutCreateWindow("Gestures in Labyrinth v1.0");
		Initializer();
		//Set handler functions
		glutDisplayFunc(drawScene);
		glutReshapeFunc (screenReshape);
		glutIdleFunc (reDraw);
		glutKeyboardFunc(controllerFunction);
		glutMainLoop();
	}

	void drawSkybox()
	{
		glDisable(GL_LIGHTING);
		glDisable(GL_LIGHT0);
		glDisable(GL_LIGHT1);

		glPushMatrix();
	
		glTranslatef(0.0f,0.0f,-1.0f + (*character.getPosition())[2]);
		glRotatef(character.getAngle(),0.0f,1.0f,0.0f);
		sky.Draw();

		glPopMatrix();

		glEnable(GL_LIGHTING);
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHT1);
	}

	void drawLabyrinth()
	{
		const Vector3f& charPos = *(character.getPosition());

		glPushMatrix();
		glRotatef(character.getAngle(),0.0f,1.0f,0.0f);
		glTranslatef(charPos[0], charPos[1], charPos[2]);
		
		labyrinth.Draw();	
		
		glPopMatrix();
	}

	void drawAllCharacters()
	{
		for (int i = 0; i < NUI_SKELETON_COUNT; i++)
		{
/*			if(kinectInterface.currentSkeleton[i])
			{
				drawCharacter(&currentSkeleton[i]);
				kinectInterface.currentSkeleton[i] = NULL;
			}*/
		}		
	}

	void drawCharacterSkeleton(Skeleton* skeleton)
	{
		const Vector4f* positionA = NULL;
		const Vector4f* positionB = NULL;	

		glDisable(GL_LIGHTING);

		glBegin(GL_LINES);			
		for (int i = 0; i < SKELETAL_NODE_COUNT - 1; i++)
		{
			positionA = skeleton->getSkeletalNode(drawIndices[2 * i])->getPosition();
			positionB = skeleton->getSkeletalNode(drawIndices[2 * i + 1])->getPosition();

			glVertex3f((*positionA)[0],(*positionA)[1],(*positionA)[2]);					
			glVertex3f((*positionB)[0],(*positionB)[1],(*positionB)[2]);	
		}		

		glEnd();		

		glEnable(GL_LIGHTING);
	}

	void drawCharacter(Skeleton* skeleton)
	{
		GLUquadricObj		*quadratic;	
		const int			slice = 36;	

		quadratic = gluNewQuadric();        
		gluQuadricNormals(quadratic, GLU_SMOOTH);   
		gluQuadricTexture(quadratic, GL_TRUE);     

		const Vector4f* position = NULL;
		const float radius = 0.04f;

		for (int i = 0; i < SKELETAL_NODE_COUNT; i++)
		{
			position = skeleton->getSkeletalNode(i)->getPosition();			
			glTranslatef((*position)[0],(*position)[1],(*position)[2]);

			if(i == nodeHead) 
			{
				gluSphere(quadratic,radius * 1.618f,slice,slice);
			}
			else 
			{
				gluSphere(quadratic,radius,slice,slice);
			}

			glTranslatef(-(*position)[0],-(*position)[1],-(*position)[2]);
		}

		gluDeleteQuadric(quadratic);
	}

	void gameInitializer()
	{
		string mapXml_s;
		char temp[256];
		ifstream mapfile;
		mapfile.open("map.xml",ifstream::in);
		while(!mapfile.eof())
		{				
			mapfile.getline(temp,256);
			mapXml_s.append(temp);
		}
		const char* mapXml;
		mapXml = mapXml_s.data();
		LabyrinthMap lmap(mapXml);

		lmap.RealizeMap();

		cellVector = lmap.getCellVector();

		Image* image = loadBMP("redbrick.bmp");		
		unsigned int texId = loadMipmappedTexture(image);
		delete image;

		image = loadBMP("Wood.bmp");		
		unsigned int texIdF = loadMipmappedTexture(image);
		delete image;

		labyrinth = Cell(vector<Wall>(),Wall(36.0f,-1.0f,-60.0f,
											 72.0f,0.1f,120.0f,
											 1.0f,1.0f,1.0f,0.0f,texIdF),0,0,0,1,0x0f);
		
		for(unsigned int i=0; i < cellVector.size(); i++)
		{
			labyrinth.AddWall(Wall(cellVector[i][0],
								   cellVector[i][1],
								   cellVector[i][2],
								   cellVector[i][3],
								   cellVector[i][4],
								   cellVector[i][5],
								   cellVector[i][6],
								   cellVector[i][7],
								   cellVector[i][8],
								   cellVector[i][9],
								   texId));
		}
	}

	void skyBoxInitializer()
	{
		Image* image; 

		image = loadBMP("skyTop.bmp");			
		sky.texId[0]  = loadMipmappedTexture(image);
		delete image;

		image = loadBMP("skyBottom.bmp");			
		sky.texId[1]  = loadMipmappedTexture(image);
		delete image;

		image = loadBMP("skyLeft.bmp");			
		sky.texId[2]  = loadMipmappedTexture(image);
		delete image;
		
		image = loadBMP("skyRight.bmp");			
		sky.texId[3]  = loadMipmappedTexture(image);
		delete image;

		image = loadBMP("skyBack.bmp");			
		sky.texId[4]  = loadMipmappedTexture(image);
		delete image;

		image = loadBMP("skyFront.bmp");			
		sky.texId[5]  = loadMipmappedTexture(image);
		delete image;

		sky.setPosition(0.0f,0.0f,0.0f);
		sky.setScale(1024,1024.0f,1024.0f);
	}

	void move(float stepping, float turn_angle)
	{
		Vector3f distance;

		distance[0] = -sin(DEG2RAD(character.getAngle()));
		distance[1] = 0.0f;
		distance[2] = cos(DEG2RAD(character.getAngle()));

		distance.normalize();
		distance *= stepping;
		
		if(!areThereCollisions(&character,labyrinth.walls,distance[0],distance[2]))
		{			
			character.move(distance);
		}	
		else 
		{
			if(collidedWall.getAngle() != 0 && !areThereCollisions(&character,labyrinth.walls,0,distance[2]))
			{
				distance[0] = 0.0f;
				character.move(distance);
			}
				
			else if(!areThereCollisions(&character,labyrinth.walls,distance[0],0)) 
			{
				distance[2] = 0.0f;
				character.move(distance);
			}			
		}

		character.turn(turn_angle);
	}
};